/**
 * @file control
 * @brief This file contains all the functions prototypes for the control
 *        firmware library.
 * @author He Shaohua (hsh15529870639@foxmail.com)
 * @date 2024/11/13
 * @version 1.0
 * @copyright Copyright (c) 2024, SNUT-Chaoyue Team
 * ****************************************************************************************************
 * @par Change Log:
 * <table>
 * <tr><th>Date         <th>Version       <th>Author      <th>Description
 * <tr><td>2024/11/13   <td> 1.0          <td>He Shaohua  <td>Creator
 * <tr><td>2025/03/13   <td> 1.1          <td>He Shaohua  <td>修改CAN总线逻辑
 * <tr><td>2025/03/26   <td> 1.2          <td>He Shaohua  <td>添加串口接收相关
 *
 * </table>
 * ==================================----How to use this driver
 * ----=====================================
 * [..]
 * @note
 *   -#
 *
 * @warning
 *   -#
 *
 * *****************************************************************************************************
 * @attention
 *
 * This document is required and please understand any bugs that may occur in
 * the document. If you want to modify this file, after the code update is
 * completed, update the version number in time, indicate the update date, and
 * fill in the changes Make sure that the submitted code does not have many
 * bugs.
 *
 * All rights reserved.
 * *****************************************************************************************************
 */
/* Includes ------------------------------------------------------------------*/
#include "control.h"
#include <driver.h>
#include "utils.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
uint32_t page[16] = {};
uint32_t can_rx_id = 0x01; // CAN ID
float motor_coder = 351.0f; // 编码器线数
uint8_t motor_stata = 0;
/* Private macro -------------------------------------------------------------*/
struct motor_data {
    float speed;
    float angle;
    float target_openloop;
    float target_speed;
    float target_position;
};
motor_data motor_can_uart_data = {};
/* Private variables ---------------------------------------------------------*/
uint8_t motor_break = 0;
uint8_t key_state = 0;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
osThreadId_t DataTaskHandle;
const osThreadAttr_t DataTask_attributes = {
        .name = "DataTask",
        .stack_size = 300,
        .priority = (osPriorityNormal),
};

osThreadId_t MotorTaskHandle;
const osThreadAttr_t MotorTask_attributes = {
        .name = "MotorTask",
        .stack_size = 800,
        .priority = (osPriorityNormal),
};

osSemaphoreId_t CANSampleBinarySemHandle;
const osSemaphoreAttr_t CANSampleBinarySem_attributes = {.name = "CANSampleBinarySem"};
/* ------------------========== TASK Deault space ==========---------------*/
/* TASK macros --------------------------------------------------------------*/
osThreadId_t DeaultTaskHandle;
const osThreadAttr_t DeaultTask_attributes = {
        .name = "DeaultTask",
        .stack_size = 300,
        .priority = (osPriorityNormal),
};

/* TASK variables --------------------------------------------------------*/
uint16_t AD_Value[2] = {};
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) {
    AD_Value[0] =hadc1.Instance->JDR1;
    AD_Value[1] =hadc1.Instance->JDR2;
}
/* TASK macro ------------------------------------------------------------*/
/* Handler macro ------------------------------------------------------------*/
/* TASK function -------------------------------------------------------- */
[[noreturn]] void DefaultTask(void *argument) {
    /* Infinite loop */
    Flash_Page1_Read(page);
    can_rx_id = page[0];
    if ( can_rx_id == 0)
        can_rx_id = 1;
    motor_coder = HEX_float(page[1]);
    can_init(can_rx_id);
    for (;;) {
        for (uint32_t i = 0; i < can_rx_id; i++) {
            HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
            osDelay(150);
            HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_SET);
            osDelay(150);
        }
        for (uint32_t i = 0; i < 8 - can_rx_id; i++) {
            osDelay(300);
        }
    }
}

/* ------------------========== TASK Motor space ==========---------------*/
/* TASK macros --------------------------------------------------------------*/
/* TASK variables --------------------------------------------------------*/
/* TASK macro ------------------------------------------------------------*/
/* TASK function -------------------------------------------------------- */
[[noreturn]] void MotorTask(void *argument) {
    uint8_t times_speed = 0;
    uint8_t can_tx_data[8] = {};
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
    __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
    HAL_ADCEx_InjectedStart(&hadc1);
    Driver_DC_Motor::DC_Motor_Control_F1 motor(GPIOB,
        GPIO_PIN_0,
        GPIOA,
        GPIO_PIN_6,
        &htim17,
        TIM_CHANNEL_1,
        &htim1,
        motor_coder);
    /* Infinite loop */
    for (;;) {
        // if (AD_Value[0] > 3165) {
        //     HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET); // 电机刹车
        //     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET); // 关闭绿灯
        //     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_SET); // 关闭蓝灯
        //     osSemaphoreDelete(CANSampleBinarySemHandle);
        //     osThreadTerminate(DeaultTaskHandle);
        //     osThreadTerminate(DataTaskHandle);
        //     HAL_FDCAN_Stop(&hfdcan1);
        //     while (true) {
        //         HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_4);
        //         osDelay(500);
        //     }
        // }
        //
        // if (AD_Value[0] > 3165) {
        //     HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET); // 电机刹车
        //     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET); // 关闭绿灯
        //     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_SET); // 关闭蓝灯
        //     osSemaphoreDelete(CANSampleBinarySemHandle);
        //     osThreadTerminate(DeaultTaskHandle);
        //     osThreadTerminate(DataTaskHandle);
        //     HAL_FDCAN_Stop(&hfdcan1);
        //     while (true) {
        //         HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_4 | GPIO_PIN_5);
        //         osDelay(500);
        //     }
        // }

        // 刹车
        if (motor_break == 0x00) {
            motor.Set_Motor_Break(true);
            motor.reset();
            motor_stata = 3;
        }
        if (motor_break == 0xff)
            motor.Set_Motor_Break(false);

        // 速度角度采样
        motor.Set_Angle_Rate();
        motor_can_uart_data.speed = motor.get_speed();
        motor_can_uart_data.angle = motor.get_angle();

        // 电机状态
        switch (motor_stata) {
            case 1:
                my_Constrain<float>(&motor_can_uart_data.target_openloop,-950,950);
                motor.Set_Motor(static_cast<int16_t>(motor_can_uart_data.target_openloop));
            break;
            case 2:
                times_speed++;
            if (times_speed > 40) {
                motor.Set_Motor_Rate(motor_can_uart_data.target_speed);
                times_speed = 0;
            }
            break;
            default:
                motor.Set_Motor_Break(true);
            break;
        }

        // CAN SEND
        auto temp = static_cast<int16_t>(motor_can_uart_data.angle);
        can_tx_data[0] = temp >> 8;
        can_tx_data[1] = temp;
        temp = static_cast<int16_t>(motor_can_uart_data.speed);
        can_tx_data[2] = temp >> 8;
        can_tx_data[3] = temp;
        can_tx_data[4] = AD_Value[0] >> 8;
        can_tx_data[5] = AD_Value[0];
        can_tx_data[6] = AD_Value[1] >> 8;
        can_tx_data[7] = AD_Value[1];
        can_send(can_rx_id, can_tx_data);
        osDelay(1);
    }
}

/* ------------------========== TASK Data space ==========---------------*/
/* TASK macros --------------------------------------------------------------*/
/* TASK variables --------------------------------------------------------*/
uint8_t can_rx_buff[8] = {};
/* TASK macro ------------------------------------------------------------*/
/* TASK function -------------------------------------------------------- */
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
    if ((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET) {
        osSemaphoreRelease(CANSampleBinarySemHandle);
    }
}

[[noreturn]] void DataTask(void *argument) {
    static FDCAN_RxHeaderTypeDef rxconf;
    /* Infinite loop */
    for (;;) {
        osSemaphoreAcquire(CANSampleBinarySemHandle, osWaitForever);
        if (uint32_t status = HAL_FDCAN_GetError(&hfdcan1); status == HAL_FDCAN_ERROR_NONE) {
            // 清空fifo
            while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0)) {
                HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &rxconf, can_rx_buff);
                if (rxconf.Identifier == can_rx_id ) {
                    if (can_rx_buff[0] == 0xFF) {
                        motor_break = 0X00;
                        const int16_t rec = can_rx_buff[3] | can_rx_buff[2] << 8;
                        motor_coder = static_cast<float>(rec);
                        osSemaphoreDelete(CANSampleBinarySemHandle);
                        osThreadTerminate(DeaultTaskHandle);
                        osThreadTerminate(MotorTaskHandle);
                        uint8_t can_tx_data[8] = {};
                        can_tx_data[0] = 0xFF;
                        can_tx_data[1] = 0x00;
                        can_tx_data[2] = can_rx_buff[2];
                        can_tx_data[3] = can_rx_buff[3];
                        can_send(can_rx_id, can_tx_data);
                        key_state = 5;
                    }else {
                        motor_stata = can_rx_buff[0];
                        motor_break = can_rx_buff[1];
                        int16_t rec = can_rx_buff[3] | can_rx_buff[2] << 8;
                        motor_can_uart_data.target_openloop = static_cast<float>(rec);
                        rec = can_rx_buff[5] | can_rx_buff[4] << 8;
                        motor_can_uart_data.target_speed = static_cast<float>(rec) / 100.0f;
                    }
                }
            }
        } else if (status == HAL_FDCAN_ERROR_TIMEOUT) {
            HAL_FDCAN_Stop(&hfdcan1);
            osDelay(1);
            status = HAL_FDCAN_Start(&hfdcan1);
            if (status == HAL_OK)
                status = HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE,0);
        }
    }
}

osThreadId_t KEYTaskHandle;
const osThreadAttr_t KEYTask_attributes = {
    .name = "KEYTask",
    .stack_size = 300,
    .priority = (osPriorityNormal),
};

[[noreturn]] void KeyTask(void *argument) {
    /* Infinite loop */
    uint16_t times = 0;
    uint32_t id_t = 1;
    for (;;) {
        if (times > 30) {
            key_state = 2;
        }
        if ( key_state == 2 || key_state == 5) {
            page[0] = can_rx_id;
            page[1] = Float_HEX(motor_coder);
            Flash_Page1_Save(page);
            __set_FAULTMASK(1);
            NVIC_SystemReset();
        }
        if ( HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_4) == GPIO_PIN_RESET ) {
            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,GPIO_PIN_RESET);
            switch (key_state) {
                case 0:
                    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_6,GPIO_PIN_SET);
                    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_4,GPIO_PIN_SET);
                    osThreadTerminate(DeaultTaskHandle);
                    key_state = 1;
                case 1:
                    can_rx_id = id_t;
                    id_t++;
                    times = 0;
                break;
                default: break;
            }
        }
        if (key_state == 1 )
            times ++;
        osDelay(100);
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,GPIO_PIN_SET);
    }
}

/* TASK macro ------------------------------------------------------------*/
void Robot_Task() {
    for (uint16_t i = 0; i < 8; i++) {
        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_5);
        osDelay(150);
    }
    osDelay(300);

    // TASK_Deault
    DeaultTaskHandle = osThreadNew(DefaultTask, nullptr, &DeaultTask_attributes);
    // TASK_Motor
    MotorTaskHandle = osThreadNew(MotorTask, nullptr, &MotorTask_attributes);
    // TASK_Data
    DataTaskHandle = osThreadNew(DataTask, nullptr, &DataTask_attributes);
    // TASK_Data
    KEYTaskHandle = osThreadNew(KeyTask, nullptr, &KEYTask_attributes);
    // Semaphore_CAN
    CANSampleBinarySemHandle = osSemaphoreNew(1, 1, &CANSampleBinarySem_attributes);
}


/**
 * @}
 */

/**
 * @}
 */

/**
 * @}
 */
/*********************************END OF FILE*********************************/
